#pragma once
#include <Math/Statistics.hpp>
#include <Math/Matrix3x3.hpp>

//to normalize coordination in an image
//based on "In Defense of Eight-Point Algorithm"
//set ori to the center of image and scale to make (average distance of all points from the origin is equal to \sqrt{2})
//will make the eight-point algorithm much more robust
//remember to restore to original coord after calculate fundamental matrix

namespace zzz{
class CoordNormalizer
{
public:
  CoordNormalizer(const Vector2ui &size):ori_(size)
  {
    ori_/=2.0;
    double distall=0;
    for (zuint r=0; r<size[0]; r++) for (zuint c=0; c<size[1]; c++)
    {
      double rr(r),cc(c);
      rr-=ori_[0];cc-=ori_[1];
      double dist=Sqrt(rr*rr+cc*cc);
      distall+=dist;
    }
    scale_=size[0]*size[1]*C_SQRT2/distall;
    CalT();
  }
  CoordNormalizer(const Vector2d &ori, const double scale):ori_(ori),scale_(scale)
  {
    CalT();
  }
  CoordNormalizer(const vector<Vector2d> &pts)
  {
    //init from a bunch of points
    //implementation as in phototourism

    zuint n=pts.size();
    //1. centroid of these points
    ori_=Mean(pts);

    //2. average distance
    double dist=0;
    for (zuint i=0; i<n; i++)
      dist+=ori_.DistTo(pts[i]);
    dist/=n;
    dist/=C_SQRT2;
    scale_=1.0/dist;

    CalT();
  }
  
  template<typename T>
  void Normalize(Vector<2,T> &ori)
  {
    ori[0]=(ori[0]-ori_[0])*scale_;
    ori[1]=(ori[1]-ori_[1])*scale_;
  }

  template<typename T>
  void Normalize(Vector<3,T> &ori)
  {
    ori[0]=(ori[0]-ori_[0])*scale_;
    ori[1]=(ori[1]-ori_[1])*scale_;
  }

  template<typename T>
  void Restore(Vector<2,T> &nor)
  {
    nor[0]=nor[0]/scale_+ori_[0];
    nor[1]=nor[1]/scale_+ori_[1];
  }

  template<typename T>
  void Restore(Vector<3,T> &nor)
  {
    nor[0]=nor[0]/scale_+ori_[0];
    nor[1]=nor[1]/scale_+ori_[1];
  }

  void Restore(Matrix3x3d &F)
  {
    F=T_.Transposed()*F*T_;
  }

  const Matrix3x3d& GetT(){return T_;}
private:
  void CalT()
  {
    //so that nor=T*ori;
    T_(0,0)=scale_;  T_(0,1)=0;      T_(0,2) = -scale_*ori_[0];
    T_(1,0)=0;      T_(1,1)=scale_;  T_(1,2) = -scale_*ori_[1];
    T_(2,0)=0;      T_(2,1)=0;      T_(2,2) = 1;
  }

  Vector2d ori_;
  double scale_;
  Matrix3x3d T_;
};
}
